#-------------------------------------------------------------------------------
# 模块名称:    基础计算模块
# 模块功能:    用于完成航海仿真中的计算地理学、射影几何学、水动力学等计算
# 模块作者:    Youan Xiao
# 创建时间:    20210412
# 更新时间:    20210518
# 版权所有：   (c) Youan@WHUT 2021
#-------------------------------------------------------------------------------

import math
#RadiusEarth = 6372795.477598    #半径二次中心
#RadiusEarth = 6378137    #地球半长轴半径
RadiusEarth = 6381372    #地球赤道半径
HalfCircumferenceEarth = math.pi * RadiusEarth #地球半周长
FullCircumferenceEarth = HalfCircumferenceEarth + HalfCircumferenceEarth  #地球半周长

def calcTrueHeading(lon1, lat1, lon2, lat2):
    """
    # 目的：依据点1和点2的经纬度，根据极坐标法，计算两点真北朝向
    # 参数：
        point p1(lon1, lat1)
        point p2(lon2, lat2)
    # 返回值：两个GPS坐标点连线的真北方位角
    """
    # 将十进制度数转化为弧度
    lon1, lat1, lon2, lat2 = map(math.radians, [lon1, lat1, lon2, lat2])
    dLon = lon2 - lon1
    y = math.sin(dLon) * math.cos(lat2)
    x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
    head = math.degrees(math.atan2(y, x))
    return (head + 360) % 360

def calcDistanceHaversine(lon1, lat1, lon2, lat2):
    """
    # 目的： 根据haversine公式计算两个以十进制度数表示的GPS坐标点的大圆距离
    # 参数：
        point p1(lon1, lat1)
        point p2(lon2, lat2)
    # 返回值：两个GPS坐标点的大圆距离，单位为 米
    """
    # 将十进制度数转化为弧度
    lon1, lat1, lon2, lat2 = map(math.radians, [lon1, lat1, lon2, lat2])
    # haversine公式
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
    c = 2 * math.asin(math.sqrt(a))
    return c * RadiusEarth
    pass

def calcTargetPosition(lon1, lat1, heading, distance):
    """
    # 目的： 已知起点经纬度和航向（方位）、航程（距离），根据极坐标法，求终点经纬度
    # 参数：
        起点经纬度: p(lon, lat)
        航向（方位）: heading(顺时针从北)
        航程（距离）: distance
    # 返回值：终点经纬度
    # 参考：https://www.movable-type.co.uk/scripts/latlong.html
            https://blog.csdn.net/diaoyanbian8143/article/details/102364521
            https://blog.csdn.net/qq_37588817/article/details/96289211
            https://blog.csdn.net/diaoyanbian8143/article/details/102364521
        lat2 = asin(sin(lat1)*cos(d/R) + cos(lat1)*sin(d/R)*cos(θ))
        lon2 = lon1 + atan2(sin(θ)*sin(d/R)*cos(lat1), cos(d/R)−sin(lat1)*sin(lat2))
        其中，
        asin = arc sin()
        d = 航程/距离。任意单位均可
        R = 地球赤道半径，单位同上 6378137m
        角距离(弧度)δ: d / R
        atan2(a,b) = arc tan(b/a)
        θ是方位角(顺时针从北)
    """
    # 将十进制度数转化为弧度
    lon1, lat1, head = map(math.radians, [lon1, lat1, heading])
    dR = distance / RadiusEarth
    lat2 = math.asin( math.sin(lat1) * math.cos(dR) + math.cos(lat1) * math.sin(dR) * math.cos(head))
    lon2 = lon1 + math.atan2( math.sin(head) * math.sin(dR) * math.cos(lat1), math.cos(dR) - math.sin(lat1) * math.sin(lat2))
    return math.degrees(lon2), math.degrees(lat2)

def calcOffsetPosition(lon1, lat1, heading, distance): # 考虑扁率
    """
    # 目的： 已知起点经纬度和航向（方位）、航程（距离），求终点经纬度
    # 要求： 考虑扁率，以及海拔高程等影响
    # 参数：
        起点经纬度: p(lon, lat)
        航向（方位）: heading(顺时针从北)
        航程（距离）: distance
    # 返回值：终点经纬度
    # 参考：https://blog.csdn.net/weixin_36380063/article/details/112928772
    """
    pass

def convertionLonlatToXY(lonlatO, lonlatP):
    """
    # 目的：依据参照点，实现经纬度到x-y之间的转换
    # 参数：
        lonlatO：坐标原点对应的经纬度
        lonlatP：需要转换的坐标点经纬度
    # 返回值：参考坐标下的x, y
    """
    distX = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatP[0], lonlatO[1])
    if (lonlatO[0] > lonlatP[0]):
        distX = -distX
    distY = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatO[0], lonlatP[1])
    if (lonlatO[1] > lonlatP[1]):
        distY = -distY
    return (distX, distY)
    
def calcMillierConvertionToXY (lon, lat):
    """
    # 目的：依据米勒投影，实现经纬度到x-y之间的转换
    # 参数：经纬度 : lon, lat
    # 返回值：米勒投影下的 x, y
    """
    W = FullCircumferenceEarth                  #平面展开，将赤道周长视为X轴
    H = HalfCircumferenceEarth                  #Y轴约等于周长一半
    mill = 2.3               #米勒投影中的一个常数，范围大约在正负2.3之间
    x, y = map(math.radians, (lon, lat)) #将经纬度从度数转换为弧度
    y = 1.25 * math.log(math.tan(0.25 * math.pi + 0.4 * y))  #这里是米勒投影的转换

    # 这里将弧度转为实际距离 ，转换结果的单位是公里
    x = (W / 2) + (W / (2 * math.pi)) * x
    y = (H / 2) - (H/(2 * mill)) * y
    return (int(round(x)), int(round(y)))

def calcMillierConvertionFromXY(x, y):
    """
    # 目的：依据米勒投影，实现x-y到经纬度之间的转换
    # 参数： x, y
    # 返回值：经纬度 : lon, lat
    """
    W = FullCircumferenceEarth                  #平面展开，将赤道周长视为X轴
    H = HalfCircumferenceEarth                  #Y轴约等于周长一半
    mill = 2.3               #米勒投影中的一个常数，范围大约在正负2.3之间
    lat = ((H/2 - y) * 2 * mill)/(1.25 * H)
    lat = ((math.atan(math.exp(lat)) - 0.25 * math.pi) * 180)/(0.4 * math.pi)
    lon = (x - W/2) * 360 / W
    return (round(lon, 7), round(lat, 7))

###==============================================================================
###  自定义坐标系转换模块
###==============================================================================
def convertionLonlatToXY(lonlatO, lonlatP):
    """
    # 目的：依据参照点，实现经纬度到x-y之间的转换
    # 参数：
        lonlatO：坐标原点对应的经纬度
        lonlatP：需要转换的坐标点经纬度
    # 返回值：参考坐标下的x, y
    """
    dist = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatP[0], lonlatP[1])
    head = calcTrueHeading(lonlatO[0], lonlatO[1], lonlatP[0], lonlatP[1])
    head = math.radians(head)
    return (dist * math.sin(head), dist * math.cos(head))

def convertionXYToLonlat(lonlatO, xyP):
    """
    # 目的：依据参照点，实现x-y到经纬度之间的转换
    # 参数：
        lonlatO：坐标原点对应的经纬度
        x, y：x-y参考坐标系下的坐标值
    # 返回值：经纬度 : lon, lat
    """
    dist = math.sqrt(xyP[0] * xyP[0] + xyP[1] * xyP[1])
    head = math.atan2(xyP[0], xyP[1])
    return calcTargetPosition(lonlatO[0], lonlatO[1], head, dist)

def convertionLonlatToXY1(lonlatO, lonlatP):
    """
    # 目的：依据参照点，实现经纬度到x-y之间的转换
    # 参数：
        lonlatO：坐标原点对应的经纬度
        lonlatP：需要转换的坐标点经纬度
    # 返回值：参考坐标下的x, y
    """
    distX = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatP[0], lonlatO[1])
    if (lonlatO[0] > lonlatP[0]):
        distX = -distX
    distY = calcDistanceHaversine(lonlatO[0], lonlatO[1], lonlatO[0], lonlatP[1])
    if (lonlatO[1] > lonlatP[1]):
        distY = -distY
    return (distX, distY)

def convertionXYToLonlat2(lonlatO, xyP):
    """
    # 目的：依据参照点，实现x-y到经纬度之间的转换
    # 参数：
        lonlatO：坐标原点对应的经纬度
        x, y：x-y参考坐标系下的坐标值
    # 返回值：经纬度 : lon, lat
    """
    lon1, lat1 = calcTargetPosition(lonlatO[0], lonlatO[1], 90, xyP[0])
    lon2, lat2 = calcTargetPosition(lon1, lat1, 0, xyP[1])
    return lon2, lat2

###==============================================================================
###  椭圆切线计算模块
###==============================================================================
import math

def calcAngleAOBold(pointA, pointO, pointB):
    """
    # 目的： 计算给定三个点AOB之间的夹角，
    # 参数：
        三个点pointA, pointO, pointB
    # 返回值：OA和OB的夹角，范围（-180~180）
    # 说明：约定长轴正向为真北（纵轴），角度计算方向为顺时钟方向，即顺时针为正，逆时针为负
    # 思路：用向量运算，所求可以看成是向量OA与OB夹角
    #       而向量OA与OB夹角的余弦=OA与OB的数量积除以它们模之积
    #       即cos(OA,OB)=OA*OB/(|OB||OB|)(这是*表示向量的点积)
    """
    x1 = pointA[0] - pointO[0]
    y1 = pointA[1] - pointO[1]
    x2 = pointB[0] - pointO[0]
    y2 = pointB[1] - pointO[1]
    x  = x1*x2+y1*y2
    y  = x1*y2-x2*y1
    d  = math.sqrt(x*x+y*y)
    bxa = x2*y1-x1*y2  # 这里约定的是左手系，非常规右手系
    if d == 0:
        return 0
    else:
        return math.copysign(math.degrees(math.acos(x/d)), bxa)
    pass

def calcAngleAOB(pointA, pointO, pointB):
    """
    # 目的： 计算给定三个点AOB之间的夹角，
    # 参数：
        三个点pointA, pointO, pointB
    # 返回值：OA和OB的夹角，范围（-180~180）
    # 说明：约定长轴正向为真北（纵轴），角度计算方向为顺时钟方向，即顺时针为正，逆时针为负
    # 思路：用向量运算，所求可以看成是向量OA与OB夹角
    #       而向量OA与OB夹角的余弦=OA与OB的数量积除以它们模之积
    #       即cos(OA,OB)=OA*OB/(|OB||OB|)(这是*表示向量的点积)
    """
    return calcAngleVectors((pointO, pointA), (pointO, pointB), 0)
    pass

def calcAngleVectors(vectorA, vectorB, mode = 0):
    """
    目的：计算给定两个向量vectorA, vectorB之间的夹角，
    参数：
        向量vectorA = (pointA1, pointA2) = (pointA1(x,y), pointA2(x,y))
        向量vectorB = (pointB1, pointB2) = (pointB1(x,y), pointB2(x,y))
        模式：mode = 0：默认模式，返回向量夹角，结果为角度
              mode = 1：弧度模式，返回向量夹角，结果为弧度
              mode = 2：方向模式，返回向量方向，结果为Bool（限制180度范围）
              mode = 3：叉积模式，返回向量叉积
    说明：约定正向为真北（纵轴），使用左手坐标系。角度计算方向为顺时钟方向，即顺时针为正，逆时针为负
    返回值：根据模式不同，返回不同结果：
        默认模式，返回向量vectorA到vectorB之间夹角的角度，范围（-180~180），顺时针为正
        弧度模式，结果类似默认模式，只是返回值为弧度
        方向模式，返回向量方向，采用左手系，True表示从向量vectorA到vectorB之间为顺时针方向
        叉积模式，返回向量叉积，这里采用的是左手系，与正常叉积相反
    思路：用向量运算，向量vectorA到vectorB之间夹角的余弦=vectorA到vectorB之间的数量积除以它们模之积
          即cos(vectorA,vectorB)=vectorA*vectorB/(|vectorA||vectorB|)(这是*表示向量的点积)
    """
    x1 = vectorA[1][0] - vectorA[0][0]
    y1 = vectorA[1][1] - vectorA[0][1]
    x2 = vectorB[1][0] - vectorB[0][0]
    y2 = vectorB[1][1] - vectorB[0][1]
    bxa = x2*y1 - x1*y2  # 叉积，这里约定的是左手系，非常规右手系
    if mode == 3:
        return bxa
    if mode == 2:
        return math.copysign(1, bxa) == 1
    x  = x1*x2+y1*y2
    y  = x1*y2-x2*y1
    d  = math.sqrt(x*x+y*y)
    if d == 0:
        rad = 0
    else:
        rad = math.copysign((math.acos(x/d)), bxa)
    pass
    if mode == 1:
        return rad
    if mode == 0:
        return math.degrees(rad)
    pass

def isSameSide(point0, point1, line):
    """
    # 目的：判断点p0和p1是否在直线line同侧
    # 参数：
        点0     : point0 (x0, y0)
        点1     : point1 (x1, y1)
        直线    : (x, y, vector (顺时针从北))
    # 返回值：bool, True or False
    """
    x0 = point0[0]
    y0 = point0[1]
    x1 = point1[0]
    y1 = point1[1]
    x  = line[0]
    y  = line[1]
    thera = math.radians(line[2])
    st = math.sin(thera)
    ct = math.cos(thera)
    # 一般直线方程：Ax+By+C=0, A = ct, B = -st
    C = y * st - x * ct
    v0 = ct * x0 - st * y0 + C
    v1 = ct * x1 - st * y1 + C
    return math.copysign(1, v0) == math.copysign(1, v1)

def calcTangentPoint4StandEllipse(Point, Ellipse):
    """
    # 目的： 计算给定观测点到标准椭圆的两个切点
    # 参数：
        观测点(x0, y0)
        椭圆参数(长轴a, 短轴b)
    # 返回值：切点相对坐标组成的列表[(x1,y1),(x2,y2)]，若列表为空，则说明无解
    # 说明：此处标准椭圆方程，约定长轴正向为真北（纵轴），角度计算方向为顺时钟方向
    """
    x0 = Point[0]
    y0 = Point[1]
    a  = Ellipse[0]
    b  = Ellipse[1]

    # 以下是加速辅助计算参数
    a2 = a * a
    a4 = a2 * a2
    b2 = b * b
    x02 = x0 * x0
    y02 = y0 * y0

    # 特殊情况处理：
    if (x02 / b2 + y02 / a2 < 1): # 此时，观测点在椭圆内部， 无解
        return []
    if (y0 == 0): # 此时，切点弦与纵轴平行，通过标准方程求解
        xt = b * b / x0
        sx = math.sqrt(x02 - b2)
        if sx == 0: # 观测点刚好在椭圆边界上，只有一个切点
            return [(xt, 0)]
        else:
            yt = a * sx / x0
            return [(xt, yt), (xt, -yt)]

    # 由极线方程，计算切点Y坐标
    k  = -(a2 * x0) / (b2 * y0)
    t  = a2 / y0

    # 韦达三参数
    A = (a2 * b2 * y02 + a4 * x02)
    AA = A + A
    B = -2 * a4 * b2 * x0
    C = (a2 - y02) * a2 * b2 * b2
    B2_4AC = B * B - 4 * A * C
    if B2_4AC < 0:  # 观测点刚好在椭圆内部，无解
        return []
    if B2_4AC == 0: # 观测点刚好在椭圆边界上，只有一个切点
        xt = -B / AA
        yt  = k * xt + t
        return [(xt, yt)]
    else:
        B2_4AC = math.sqrt(B2_4AC)
        xt1 = (-B + B2_4AC)/ AA
        yt1  = k * xt1 + t
        xt2 = (-B - B2_4AC)/ AA
        yt2  = k * xt2 + t
        return [(xt1, yt1), (xt2, yt2)]

def doTranslationRotationTransformation(Point, Center, angle):
    """
    # 目的： 计算标准航海坐标系下给定观测点P到经过平移和旋转后的新坐标点
    # 参数：
        观测点(x0, y0)
        中心点(Xe，Ye)
        旋转角度angle
    # 返回值：新的坐标点(x,y)，以中心点(Xe，Ye)为中心，以真北为0度
    # 说明：约定角度计算方向为顺时钟方向
    """
    x0 = Point[0]
    y0 = Point[1]
    xe = Center[0]
    ye = Center[1]
    rd = math.radians(angle)

    # 平移后的点
    xx = x0 - xe
    yy = y0 - ye
    ca = math.cos(rd)
    sa = math.sin(rd)
    return (xx * ca - yy * sa, xx * sa + yy * ca)

def doInverseTranslationRotationTransformation(Point, Center, angle):
    """
    # 目的： 计算标准椭圆坐标系下给定点经逆变换后，到标准航海坐标系下的坐标点
    # 参数：
        标准椭圆坐标系下给定点(x0, y0)
        中心点(Xe，Ye)
        旋转角度angle（真北角）
    # 返回值：标准坐标点(x,y)，
    # 说明：约定角度计算方向为顺时钟方向，以真北为0度
    """
    x0 = Point[0]
    y0 = Point[1]
    xe = Center[0]
    ye = Center[1]
    rd = math.radians(angle)

    # 平移后的点
    ca = math.cos(rd)
    sa = math.sin(rd)
    return (x0 * ca + y0 * sa + xe, - x0 * sa + y0 * ca + ye)

def calcTangentPoint4GeneralEllipse(Point, Ellipse):
    """
    # 目的： 计算给定观测点到一般化任意椭圆的两个切点和椭圆中心点的夹角
    # 参数：
        观测点Point(x0, y0)
        椭圆参数(长轴a, 短轴b, 中心点Xe，Ye, 长轴真北朝向head)
    # 返回值：由观测点到两个切点的夹角。以及两个切点的坐标组成的列表[(x1,y1),(x2,y2)]
    # 说明：此处一般化椭圆方程，约定角度计算方向为顺时钟方向
    """
    # 0、读取参数
    x0 = Point[0]
    y0 = Point[1]
    a  = Ellipse[0]
    b  = Ellipse[1]
    xe = Ellipse[2]
    ye = Ellipse[3]
    head = Ellipse[4]
    #print("Point, Ellipse = ", Point, Ellipse, "in calcTangentPoint4GeneralEllipse")
    # 1、通过仿射变换，将初始平面上观测点P映射为标准椭圆平面上
    x, y = doTranslationRotationTransformation((x0,y0), (xe,ye), head)

    # 2、计算标准椭圆平面上的切点
    TangentPoints = calcTangentPoint4StandEllipse((x, y), (a, b))

    if len(TangentPoints) < 2:
        return []

    # 3、计算标准椭圆平面上的两夹角(需要排序).
    pointO = (0, 0)
    pointP = (x, y)
    pointTP = TangentPoints[0]
    pointTN = TangentPoints[1]
    angleTP = calcAngleAOB(pointO, pointP, pointTP)
    angleTN = calcAngleAOB(pointO, pointP, pointTN)

    # 4、将标准椭圆平面上的切点映射回一般平面
    NewTP = doInverseTranslationRotationTransformation(pointTP, (xe, ye), head)
    NewTN = doInverseTranslationRotationTransformation(pointTN, (xe, ye), head)
    return [angleTP, angleTN, NewTP, NewTN]
    pass

def getPointOnCircle(circle, angle):
    angle = math.radians(angle)
    xc = circle[0]
    yc = circle[1]
    ra = circle[2]
    return (xc + ra * math.sin(angle), yc + ra * math.cos(angle))
    pass

def calcLineCircleIntersect(point, vector, circle):
    """
    # 目的：依据通过点point，方位角为vector的直线，与圆circle的交点
    # 参数：
        起点     : point (x0, y0)
        直线方位 : vector (顺时针从北)
        目标圆   : circle (xc, yc, radius)
    # 返回值：直线与圆的交点与垂线夹角，直线与圆的交点列表（0个，1个，2个）
    """
    x0 = point[0]
    y0 = point[1]
    xc = circle[0]
    yc = circle[1]
    ra = circle[2]
    thera1 = math.radians(vector)
    st = math.sin(thera1)
    ct = math.cos(thera1)
    E = y0 * st - x0 * ct
    m = xc * ct - yc * st + E
    if abs(m) > ra:
        return None, []
    if abs(m) == ra:
        return 0, [getPointOnCircle(circle, vector - 90)]
    if abs(m) < ra:
        #print('m, ra: ', m, ra)
        beta = math.acos(m / ra)
        beta = math.degrees(beta)
        return [vector - 90 + beta, vector - 90 - beta], [getPointOnCircle(circle, vector - 90 + beta), getPointOnCircle(circle, vector - 90 - beta)]
        #return beta, [getPointOnCircle(circle, vector - 90 + beta), getPointOnCircle(circle, vector - 90 - beta)]
    pass


